#include <cfloat>
#include <cmath>
#include <cassert>
#include "Curve.h"

Beachline *Beachline::New(const Site& site)
{
	if(site.isPoint_)
		return new Parabola(site);
	else
		return new Polyline(site);
}

Vector *Beachline::getDivision(double low, double high, double time, int& size) const
{
	if(equalDouble(time, site_.p0_.x)) 
		return NULL;
	size = (high-low) / CURVESTEP;
	if(size < 2) return NULL;
	int downsize = size / 2;
	Vector *varray = new Vector[size];
	for(int i=0; i < downsize; i ++)
	{
		varray[i].x = evaluateXAt(low, time);
		varray[i].y = low;
		low += CURVESTEP;
	}
	for(int i=size-1; i >= downsize; i --)
	{
		varray[i].x = evaluateXAt(high, time);
		varray[i].y = high;
		high -= CURVESTEP;
	}
	return varray;
}

double Parabola::evaluateXAt(double y, double directrix) const
{
	assert(directrix >= site_.p0_.x);
	//wy test
	double t = 0.0;
	if(equalDouble(directrix, site_.p0_.x))
//		return site_.p0_.x;
		t = 1e-5;

	double x = y-site_.p0_.y;
	x *= x;
	x /= site_.p0_.x-directrix-t;
	return (x + site_.p0_.x + directrix) / 2;
}

bool Parabola::intersect(const Beachline *peer, double directrix,
						 Vector& p1, Vector& p2) const
{
	if (directrix<=site_.p0_.x)
	{
		p1.x = DBL_MAX;
		p1.y = DBL_MAX;
		p2 = p1;
		return false;

	}
	if(!peer->isSitePoint()){
		return !peer->intersect(this, directrix, p1, p2);
	}

	const Parabola *para = (const Parabola *)peer;
	Vector focus = para->getFocus();

	if (directrix<=focus.x)
	{
		p1.x = DBL_MAX;
		p1.y = DBL_MAX;
		p2 = p1;
		if (equalDouble(site_.p0_.x,focus.x))
		{
			focus.x += 1e-5;
		}
		return (site_.p0_.x < focus.x);
	}

	if (equalDouble(directrix,site_.p0_.x))  //wy change 
	{
		p1.y = site_.p0_.y;
		p1.x = peer->evaluateXAt(p1.y, directrix);
		p2 = p1;
	}
	else if (equalDouble(directrix, focus.x))  //wy change
	{
		p1.y = focus.y;

		p1.x = evaluateXAt(p1.y, directrix);
		p2 = p1;
	}
	else
	{
		intersect(peer, directrix, p1.y, p2.y);
		p1.x = evaluateXAt(p1.y, directrix);
		p2.x = evaluateXAt(p2.y, directrix);
	}

	if (equalDouble(site_.p0_.x,focus.x))
	{
		focus.x += 1e-5;
	}

	return (site_.p0_.x < focus.x);  // wy test
}

bool Parabola::intersect(const Beachline *peer, double directrix,
						 double& y1, double& y2) const
{
	if (directrix<=site_.p0_.x)
	{
		y1 = DBL_MAX;
		y2 = DBL_MAX;
		return false;
	}

	if(!peer->isSitePoint()){
		return !peer->intersect(this, directrix, y1, y2);
	}

	const Parabola *para = (const Parabola *)peer;
	Vector focus = para->getFocus();

	if (directrix<=focus.x)
	{
		y1 = DBL_MAX;
		y2 = DBL_MAX;
		return (site_.p0_.x < focus.x);
	}

	//wy test 
	if (equalDouble(site_.p0_.x, focus.x))
	{
		//y1 = (site_.p0_.y+focus.y)/2;
		//y2 = y1;
		//return (site_.p0_.x < focus.x);
		focus.x += 1e-5;
	}

	double p1 = (site_.p0_.x-directrix)/2;
	double p2 = (focus.x-directrix)/2;

	double sbX2X1 = p2 - p1;
	double dvY1SbX2X1 = site_.p0_.y / sbX2X1;
	double dvY2SbX2X1 = focus.y / sbX2X1;
	double sqrDvY1Y2SbX2X1 = (dvY1SbX2X1 - dvY2SbX2X1);
	sqrDvY1Y2SbX2X1 *= sqrDvY1Y2SbX2X1;

	double p1p2 = p1 * p2;
	double temp1 = p2*dvY1SbX2X1 - p1*dvY2SbX2X1;
	double temp2 = sqrt((sqrDvY1Y2SbX2X1 + 4) * p1p2);

	y1 = temp1+temp2;
	y2 = temp1-temp2;

	return (site_.p0_.x < focus.x);
}

Polyline::Polyline(const Site& p)
: Beachline(p)
{
	if(equalDouble(site_.p0_.x, site_.p1_.x)) return;
	if(equalDouble(site_.p0_.y, site_.p1_.y))
	{
		above_tang = below_tang = 1.0;
		return;
	}
	double len = (site_.p0_-site_.p1_).getLength();
	double sin = fabs(site_.p0_.x-site_.p1_.x) / len;
	double cos = fabs(site_.p0_.y-site_.p1_.y) / len;
	if(site_.p0_.y < site_.p1_.y)
	{
		below_tang = sin / (1+cos);
		above_tang = 1.0 / below_tang;
	}else{
		above_tang = sin / (1+cos);
		below_tang = 1.0 / above_tang;
	}
}

double Polyline::evaluateXAt(double y, double directrix) const
{
	if(equalDouble(site_.p0_.x, site_.p1_.x)) return (directrix+site_.p0_.x) / 2;
	double dy = (site_.p1_.y*(site_.p0_.x-directrix)+site_.p0_.y*(directrix-site_.p1_.x)) / (site_.p0_.x-site_.p1_.x);
	if(equalDouble(y, dy))
		return directrix;
	else if(y > dy)
	{
		return(directrix - (y-dy)*above_tang);
	}else
	{
		return(directrix - (dy-y)*below_tang);
	}
}

bool Polyline::intersect(const Beachline *peer, double directrix,
						 Vector& p1, Vector& p2) const
{
	double y1, y2;
	bool res = intersect(peer, directrix, y1, y2);
	p1.x = evaluateXAt(y1, directrix);
	p1.y = y1;
	p2.x = evaluateXAt(y2, directrix);
	p2.y = y2;

	return res;
}

bool Polyline::intersect(const Beachline *peer, double directrix,
						 double& y1, double& y2) const
{
	if(equalDouble(site_.p0_.x, site_.p1_.x))
	{
		double dx = (directrix+site_.p0_.x) / 2;
		if(peer->isSitePoint())
		{
			Vector p = ((const Parabola *)peer)->getFocus();
			double vy = sqrt((directrix-dx)*(directrix-dx) - (dx-p.x)*(dx-p.x));
			y1 = p.y + vy;
			y2 = p.y - vy;
			return true;
		}else{
			const Polyline *poly = (const Polyline *)peer;
			if(poly->isVertical()) assert(0);
			return !poly->intersect(this, directrix, y1, y2);
		}
	}else{
		double dy = (site_.p1_.y*(site_.p0_.x-directrix)+site_.p0_.y*(directrix-site_.p1_.x)) / (site_.p0_.x-site_.p1_.x);
		if(peer->isSitePoint())
		{
			Vector p = ((const Parabola *)peer)->getFocus();
			if(site_.p0_==p)
			{
				Vector dp(directrix, dy);
				double len = (p-dp).getLength();
				y1 = dy + len;
				y2 = dy - len;
				return false;
			}else if(site_.p1_==p)
			{
				Vector dp(directrix, dy);
				double len = (p-dp).getLength();
				y1 = dy + len;
				y2 = dy - len;
				return true;
			}
			else if(equalDouble(directrix, p.x))
			{
				y1 = y2 = p.y;
				return false;
			}
			bool by1 = false, by2 = false, res = false;
			double b = p.y + above_tang*(directrix-p.x);
			double c = (directrix-p.x)*(2*above_tang*dy+directrix-p.x)+p.y*p.y;
			double delta = b*b - c;
			if(delta>=0)
			{
				delta = sqrt(delta);
				y1 = b + delta;
				y2 = b - delta;
				if(y1 >= dy) by1 = true;
				if(y2 >= dy) by2 = true;
				if(equalDouble(delta, 0.0)) by2 = false;
			}
			if(by1 && by2) return true;
			if(!by1 && !by2) res = true;
			b = p.y - below_tang*(directrix-p.x);
			c = (directrix-p.x)*(-2*below_tang*dy+directrix-p.x)+p.y*p.y;
			delta = b*b - c;
			if(delta>=0)
			{
				delta = sqrt(delta);
				if(!by1){ y1 = b + delta; if(y1 <= dy) by1 = true; }
				if(!by2){ y2 = b - delta; if(y2 <= dy) by2 = true; }
			}
			if(!by1) y1 = DBL_MAX;
			if(!by2) y2 = DBL_MAX;
			return res;
		}else{
			const Polyline *poly = (const Polyline *)peer;
			if(poly->isVertical())
			{
				double xd = (directrix-poly->getStartPoint().x) / 2;
				y1 = dy + xd*below_tang;
				y2 = dy - xd*above_tang;
				return false;
			}else{
				Vector p0 = poly->getStartPoint();
				Vector p1 = poly->getEndPoint();
				double dy2 = (p1.y*(p0.x-directrix)+p0.y*(directrix-p1.x)) / (p0.x-p1.x);
				if(equalDouble(dy, dy2))
				{
					y1 = dy;
					y2 = y1;
					return false;
				}else{
					bool bIntersected, bToIntersect;
					double c1 = (site_.p0_-p0).Cross(site_.p0_-p1);
					double c2 = (site_.p1_-p0).Cross(site_.p1_-p1);
					double c3 = (p0-site_.p0_).Cross(p0-site_.p1_);
					double c4 = (p1-site_.p0_).Cross(p1-site_.p1_);
					if((c1>0&&c2>0) || (c1<0&&c2<0))
						bIntersected = false;
					else
						bIntersected = true;
					if((c3>0&&c4>0) || (c3<0&&c4<0))
						bToIntersect = false;
					else
						bToIntersect = true;
					if(dy > dy2)
					{
						double above_tang2 = poly->getAboveTang();
						if(!bIntersected && !bToIntersect)
						{
							if(equalDouble(site_.p0_.x, p0.x))
							{
								y1 = (below_tang*dy + above_tang2*dy2) / (below_tang+above_tang2);
								y2 = y1;
								return true;
							}else if(site_.p0_.x < p0.x)
							{
								y1 = (below_tang*dy + above_tang2*dy2) / (below_tang+above_tang2);
								y2 = (above_tang*dy2 - above_tang2*dy) / (above_tang-above_tang2);
								return true;
							}else
							{
								y1 = (above_tang*dy - above_tang2*dy2) / (above_tang-above_tang2);
								y2 = (below_tang*dy + above_tang2*dy2) / (below_tang+above_tang2);
								return false;
							}
						}else if(bIntersected)
						{
							if(c3 > 0)
							{
								y1 = (above_tang*dy - above_tang2*dy2) / (above_tang-above_tang2);
								y2 = y1;
								return true;
							}else{
								y1 = (below_tang*dy + above_tang2*dy2) / (below_tang+above_tang2);
								y2 = (above_tang*dy2 - above_tang2*dy) / (above_tang-above_tang2);
								return true;
							}
						}else
						{
							if(c1 > 0)
							{
								y1 = (above_tang*dy - above_tang2*dy2) / (above_tang-above_tang2);
								y2 = (below_tang*dy + above_tang2*dy2) / (below_tang+above_tang2);
								return false;
							}else{
								y1 = (above_tang*dy2 - above_tang2*dy) / (above_tang-above_tang2);
								y2 = y1;
								return false;
							}
						}
					}else
					{
						double below_tang2 = poly->getBelowTang();
						if(!bIntersected && !bToIntersect)
						{
							if(equalDouble(site_.p0_.x, p0.x))
							{
								y1 = (above_tang*dy + below_tang2*dy2) / (above_tang+below_tang2);
								y2 = y1;
								return true;
							}else if(site_.p0_.x < p0.x)
							{
								y1 = (below_tang*dy2 - below_tang2*dy) / (below_tang-below_tang2);
								y2 = (above_tang*dy + below_tang2*dy2) / (above_tang+below_tang2);
								return true;
							}else
							{
								y1 = (above_tang*dy + below_tang2*dy2) / (above_tang+below_tang2);
								y2 = (below_tang*dy - below_tang2*dy2) / (below_tang-below_tang2);
								return false;
							}
						}else if(bIntersected)
						{
							if(c3 > 0)
							{
								y1 = (below_tang*dy2 - below_tang2*dy) / (below_tang-below_tang2);
								y2 = (above_tang*dy + below_tang2*dy2) / (above_tang+below_tang2);
								return true;
							}else{
								y1 = (below_tang*dy - below_tang2*dy2) / (below_tang-below_tang2);
								y2 = y1;
								return true;
							}
						}else
						{
							if(c1 > 0)
							{
								y1 = (below_tang*dy2 - below_tang2*dy) / (below_tang-below_tang2);
								y2 = y1;
								return true;
							}else{
								y1 = (above_tang*dy + below_tang2*dy2) / (above_tang+below_tang2);
								y2 = (below_tang*dy - below_tang2*dy2) / (below_tang-below_tang2);
								return false;
							}
						}
					}
				}
			}
		}
	}
}

ParaEdge::ParaEdge(const Vector& f, const Vector& p0, const Vector& p1)
:focus_(f)
{
	Vector v10 = p1 - p0;
	double len = (f-p0).Dot(v10) / v10.getSquaredLength();
	v10 = v10 * len;
	dest_ = v10 + p0;
	length_ = (dest_-focus_).getLength();
}

Vector *ParaEdge::getDivision(const Vector& p0, const Vector& p1, int& size)
{
	Vector fd = focus_ - dest_;
	Vector np0 = p0 - fd*(fd.Dot(p0-dest_)/fd.getSquaredLength());
	Vector np1 = p1 - fd*(fd.Dot(p1-dest_)/fd.getSquaredLength());
	double len = (np1-np0).getLength();
	size = len / CURVESTEP;
	if(size < 2) return NULL;
	int downsize = size / 2;
	Vector dp = (np1-np0) * CURVESTEP / len; 
	Vector *varray = new Vector[size];
	Vector pt = np0;
	for(int i=0; i < downsize; i ++)
	{
		Vector vp = pt - dest_;
		double dist_ = (vp.getSquaredLength()/length_+length_) / 2;
		Vector op = (focus_*dist_+dest_*(length_-dist_)) / length_;
		Vector rp = op + vp;
		varray[i].x = rp.x;
		varray[i].y = rp.y;
		pt = pt + dp;
	}
	pt = np1;
	for(int i=size-1; i >= downsize; i --)
	{
		Vector vp = pt - dest_;
		double dist_ = (vp.getSquaredLength()/length_+length_) / 2;
		Vector op = (focus_*dist_+dest_*(length_-dist_)) / length_;
		Vector rp = op + vp;
		varray[i].x = rp.x;
		varray[i].y = rp.y;
		pt = pt - dp;
	}
	return varray;
}

